#include "at32f413_board.h"
#include "at32f413_clock.h"

#include "i2c_user.h"
#include "mt6701.h"
#include "motor.h"
#include "current_sense.h"
#include "speed_timer.h"

#include "u8g2_hal.h"
#include "fram.h"
#include "btn_timer.h"
#include "debug_serial.h"
#include "debug_led.h"

extern volatile ControlMode_t CURR_MODE;
extern volatile float setVelocity;

extern volatile float setAngle;
extern volatile float current_target;
extern volatile float velocity;

extern volatile float currentD, currentQ;

extern volatile float zeroElectricAngleOffset;
extern volatile uint32_t ms_tick;

u8g2_t u8g2;

void dwt_init(void)
{
  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  /**
    * On Cortex-M7 core there is a LAR register in DWT domain.
    * Any time we need to setup DWT registers, we MUST write
    * 0xC5ACCE55 into LAR first. LAR means Lock Access Register.
    */
  DWT->CYCCNT = 0;
  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

void debug_io_init(void)
{
  // PB10
  gpio_init_type gpio_initsturct;
  gpio_default_para_init(&gpio_initsturct);

  crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE);
  gpio_initsturct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
  gpio_initsturct.gpio_mode = GPIO_MODE_OUTPUT;
  gpio_initsturct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
  gpio_initsturct.gpio_pins = GPIO_PINS_10;
  gpio_initsturct.gpio_pull = GPIO_PULL_NONE;
  gpio_init(GPIOB, &gpio_initsturct);
}

void oled_init(void)
{
  i2c_user_init();
  u8g2_Setup_ssd1306_i2c_128x64_noname_f(
    &u8g2,
    U8G2_R0,
    //u8x8_byte_sw_i2c,
    u8x8_byte_i2c,
    u8x8_gpio_and_delay);  // init u8g2 structure
  u8x8_SetI2CAddress(&u8g2.u8x8, 0x78);

  u8g2_InitDisplay(&u8g2); // send init sequence to the display, display is in sleep mode after this,
  u8g2_SetPowerSave(&u8g2, 0); // wake up display
  u8g2_SetContrast(&u8g2, 0xff);
  u8g2_ClearDisplay(&u8g2);

  u8g2_SetFont(&u8g2, u8g2_font_t0_11_tf);
  u8g2_DrawStr(&u8g2, 0, 8, "AT32F413 FOC DRIVER");
  u8g2_DrawStr(&u8g2, 0, 16, "VER 1.0");
  u8g2_DrawStr(&u8g2, 0, 24, "System Loading...");

  char buf[32] = {0};
  uint16_t boot_cnt = 0x0;
  fram_read_reg(FRAM_ADDR_BOOTCNT, 2, (uint8_t*)&boot_cnt);
  debug_printf("FRAM BOOT CNT:%d\n", boot_cnt);
  snprintf(buf, 32, "BOOT CNT: %d", boot_cnt);
  u8g2_DrawStr(&u8g2, 0, 34, buf);
  u8g2_SendBuffer(&u8g2);

  boot_cnt++;
  fram_write_reg(FRAM_ADDR_BOOTCNT, 2, (uint8_t*)&boot_cnt);
}

void draw_system_state(void)
{
  u8g2_ClearBuffer(&u8g2);
  char buf[32];
  switch(CURR_MODE)
  {
  case VELOCITY:
    strncpy(buf, "mode: VELOCITY", 32);
    break;
  case OPENLOOP:
    strncpy(buf, "mode: OPENLOOP", 32);
    break;
  case ANGLE:
    strncpy(buf, "mode: ANGLE", 32);
    break;
  case CURRENT:
    strncpy(buf, "mode: CURRENT", 32);
    break;
  default:
    strncpy(buf, "mode: UNDEFINED", 32);
    break;
  }
  u8g2_DrawStr(&u8g2, 0, 8, buf);
  snprintf(buf, 32, "zeroElectric: %.2f", zeroElectricAngleOffset);
  u8g2_DrawStr(&u8g2, 0, 16, buf);
  snprintf(buf, 32, "setVelocity: %.2f", setVelocity);
  u8g2_DrawStr(&u8g2, 0, 23, buf);
  snprintf(buf, 32, "velocity: %.2f", velocity);
  u8g2_DrawStr(&u8g2, 0, 31, buf);
  snprintf(buf, 32, "currentQ: %.2f", currentQ);
  u8g2_DrawStr(&u8g2, 0, 39, buf);
  u8g2_SendBuffer(&u8g2);
}

int main(void)
{
  system_clock_config();
  nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);

  at32_board_init();
  debug_port_init(921600);
  crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE);

  dwt_init();
  debug_io_init();
  fram_init();
  oled_init();
  debug_led_init();
  user_button_init();

  mt6701_Init();
  encoder_VarInit();
  eg2133_Init();

  AlignSensor();
//  delay_ms(1000);
  LowSideCurrentSense_Init();

  debug_printf("clock = %d\r\n", system_core_clock);
  speed_timer_init();

  int16_t fram_velocity = 0;
  fram_read_reg(FRAM_ADDR_SETVELOCITY, 2, (uint8_t*)&fram_velocity);
  fram_velocity = CONSTRAINT(fram_velocity / 100.0, -180, 180);
  setVelocity = fram_velocity;

  CURR_MODE = VELOCITY;
//  CURR_MODE = OPENLOOP;
//  CURR_MODE = ANGLE;
//  CURR_MODE = CURRENT;

  static int16_t _save_velocity = 0;
  static float debug_data[2] = {0};
  static uint32_t last_tick = 0;
  while(1)
  {
    if(sw2_clicked_flag)
    {
      sw2_clicked_flag = 0;
//      setAngle += 0.1;
//      setAngle = CONSTRAINT(setAngle, -2 * _PI, 2 * PI);
      setVelocity -= 10;
      setVelocity = CONSTRAINT(setVelocity, -180, 180);
      _save_velocity = setVelocity * 100;
      fram_write_reg(FRAM_ADDR_SETVELOCITY, 2, (uint8_t*)&_save_velocity);
    }
    if(sw3_clicked_flag)
    {
      sw3_clicked_flag = 0;
//      setAngle -= 0.1;
//      setAngle = CONSTRAINT(setAngle, -2 * _PI, 2 * PI);
      setVelocity += 10;
      setVelocity = CONSTRAINT(setVelocity, -180, 180);
      _save_velocity = setVelocity * 100;
      fram_write_reg(FRAM_ADDR_SETVELOCITY, 2, (uint8_t*)&_save_velocity);
    }
    if(ms_tick - last_tick > 100)
    {
      last_tick = ms_tick;
      draw_system_state();
    }
//    encoder_Update();
//    printf("%.2f\r\n", encoder_GetFullAngle());
//    VelocityOpenLoopTick(5);
//    delay_ms(1);
//    printf("%.2f,%.2f\r\n", currentQ, currentD);
//    printf("%.2f,%.2f\r\n", current_target, velocity);
//    debug_data[0] = current_target;
//    debug_data[1] = velocity;
//    debug_port_write_hex((uint8_t*)debug_data, 8);
//    delay_us(100);
  }
}
